#pragma once
#include <zGraphicsConfig.hpp>
#include "../Resource/Mesh/SimpleMesh.hpp"
#include <Math/Vector3.hpp>
#include <Math/Statistics.hpp>
#include <Math/Matrix3x3.hpp>
#include <Math/Array2.hpp>

namespace zzz{
template<zuint PN> class Mesh;
class GeometryHelper {
public:
template <typename T>
static Vector<3,T> RotateVector3(const VectorBase<3,T> &v, const VectorBase<3,T> &normal, const T angle)
{
  Vector<3,T> tmp=normal*v.Dot(normal);
  return (v-tmp)*cos(angle)+normal.Cross(v)*sin(angle)+tmp;
}

template <typename T, typename T1>
static void Barycentric(const VectorBase<2,T> &p, const VectorBase<2,T> &t1, const VectorBase<2,T> &t2, const VectorBase<2,T> &t3, VectorBase<3,T1> &b)
{
  VectorBase<2,T> v1=t1-t3;
  VectorBase<2,T> v2=t2-t3;
  T1 d=1.0/(v1[0]*v2[1]-v1[1]*v2[0]);
  VectorBase<2,T> x=p-t3;
  b[0]=(x[0]*v2[1]-x[1]*v2[0])*d;
  b[1]=(x[1]*v1[0]-x[0]*v1[1])*d;
  b[2]=1.0-b[0]-b[1];
}

template <int N, typename T>
static T TriangleArea(const VectorBase<N,T> &t1, const VectorBase<N,T> &t2, const VectorBase<N,T> &t3)
{
  T a=t1.DistTo(t2);
  T b=t2.DistTo(t3);
  T c=t3.DistTo(t1);
  T v=(T)((a+b+c)/2.0);
  return Sqrt<T>(v*(v-a)*(v-b)*(v-c));
}

template <typename T>
static T TriangleArea(const VectorBase<2,T> &t1, const VectorBase<2,T> &t2, const VectorBase<2,T> &t3)
{
  return 0.5 * Abs(Cross(t2 - t1, t3 - t1));
}

template <typename T>
static T TriangleArea(const VectorBase<3,T> &t1, const VectorBase<3,T> &t2, const VectorBase<3,T> &t3)
{
  return 0.5 * Cross(t2 - t1, t3 - t1).Len();
}

//return if p is on the left of l1->l2
template <typename T>
static bool IsOnLeft(const VectorBase<2,T> &p, const VectorBase<2,T> &l1, const VectorBase<2,T> &l2)
{
  T x = Cross(l2 - l1, p - l1);
  return x >= -TINY_EPSILON;
}

//return if p1 and p2 are on the same side of the line through l1 l2
template <typename T>
static bool IsSameSide2D(const VectorBase<2,T> &p1, const VectorBase<2,T> &p2, const VectorBase<2,T> &l1, const VectorBase<2,T> &l2)
{
  T x = ((p1[0]-l1[0]) * (l2[1]-l1[1]) - (l2[0]-l1[0]) * (p1[1]-l1[1]))
    * ((p2[0]-l1[0]) * (l2[1]-l1[1]) - (l2[0]-l1[0]) * (p2[1]-l1[1]));
  return x >= -TINY_EPSILON;
}

//return if p is inside triangle t1 t2 t3
template <typename T>
static bool PointInTriangle2D(const Vector<2,T> &t1, const Vector<2,T> &t2, const Vector<2,T> &t3, const Vector<2,T> &p)
{
  if (EPSILON < t1[0]-p[0] && EPSILON < t2[0]-p[0] && EPSILON < t3[0]-p[0]) return false;
  if (p[0]-t1[0] > EPSILON && p[0]-t2[0] > EPSILON && p[0]-t3[0] > EPSILON) return false;
  if (EPSILON < t1[1]-p[1] && EPSILON < t2[1]-p[1] && EPSILON < t3[1]-p[1]) return false;
  if (p[1]-t1[1] > EPSILON && p[1]-t2[1] > EPSILON && p[1]-t3[1] > EPSILON) return false;
  return IsSameSide2D(p,t1,t2,t3) && IsSameSide2D(p,t2,t1,t3) && IsSameSide2D(p,t3,t1,t2);

}

template <typename T>
static bool IsSameSide3D(const VectorBase<3,T> &p1, const VectorBase<3,T> &p2, const VectorBase<3,T> &l1, const VectorBase<3,T> &l2)
{
  Vector<3,T> a(l2-l1),b(p1-l1),c(p2-l1);
  return FastDot(a.Cross(b),a.Cross(c)) > 0;
}

//return if p is inside triangle t1 t2 t3
template <typename T>
static bool PointInTriangle3D(const VectorBase<3,T> &t1, const VectorBase<3,T> &t2, const VectorBase<3,T> &t3, const VectorBase<3,T> &p)
{
  return IsSameSide3D(p,t1,t2,t3) && IsSameSide3D(p,t2,t1,t3) && IsSameSide3D(p,t3,t1,t2);

}


//Ray intersect Triangle
template<typename T, typename T1>
static bool RayInTriangle(const VectorBase<3,T> &t0, const VectorBase<3,T> &t1, const VectorBase<3,T> &t2,\
              const VectorBase<3,T> &orig, const VectorBase<3,T> &dir, \
              T1 &dist, Vector<3,T1> &bary)
{
  Vector<3,T> tvec, pvec, qvec;

  /* find vectors for two edges sharing vert0 */
  Vector<3,T> edge1=t1-t0;
  Vector<3,T> edge2=t2-t0;

  /* begin calculating determinant - also used to calculate U parameter */
  Cross(dir,edge2,pvec);

  /* if determinant is near zero, ray lies in plane of triangle */
  T det = FastDot(edge1, pvec);

#ifdef CULL_BACK_FACE
  if (det < EPSILON) return false;

  /* calculate distance from vert0 to ray origin */
  tvec = orig - t0;

  /* calculate U parameter and test bounds */
  bary[0] = Dot(tvec, pvec);
  if (bary[0] < 0.0 || bary[0] > det) return false;

  /* prepare to test V parameter */
  Cross(tvec,edge1,qvec);

  /* calculate V parameter and test bounds */
  bary[1] = Dot(dir, qvec);
  if (bary[1] < 0.0 || bary[0] + bary[1] > det) return false;

  /* calculate t, scale parameters, ray intersects triangle */
  dist = Dot(edge2, qvec);
  T inv_det = 1.0 / det;
  dist *= inv_det;
  bary[0] *= inv_det;
  bary[1] *= inv_det;
#else                    /* the non-culling branch */
  if (det > -EPSILON && det < EPSILON) return false;
  T inv_det = 1.0 / det;

  /* calculate distance from vert0 to ray origin */
  tvec=orig-t0;

  /* calculate U parameter and test bounds */
  bary[0] = FastDot(tvec, pvec) * inv_det;
  if (bary[0] < -EPSILON || bary[0] > 1.0+EPSILON) return false;

  /* prepare to test V parameter */
  Cross(tvec,edge1,qvec);

  /* calculate V parameter and test bounds */
  bary[1] = FastDot(dir, qvec) * inv_det;
  if (bary[1] < -EPSILON || bary[0] + bary[1] > 1.0+EPSILON) return false;

  /* calculate t, ray intersects triangle */
  dist = FastDot(edge2, qvec) * inv_det;
#endif
  bary[2] = 1.0 - bary[0] - bary[1];
  return true;
}

//decide if a polygon is a convex
//just cross product each adjacent edges, convex will have the same sign
//return 1:counter-closewise convex, -1:closewise convex, 0:not convex
template <typename T>
static int IsConvex(const vector<Vector<2,T> > &points)
{
  T dir=Sign(Cross(points[1]-points[0],points.back()-points[0]));
  int psize=points.size();
  for (int i=1; i<psize; i++)
  {
    T thisdir=Sign(Cross(points[(i+1)%psize]-points[i],points[(i+psize-1)%psize]-points[i]));
    if (thisdir!=dir) return 0;
  }
  return int(dir);
}

//decide vertices ordering for a polygon
//it will detect if it is a convex first, if not a further algorithm for concave will be used
//return 1:counter-closewise, -1:closewise
template <typename T>
static int PolygonOrdering(const vector<Vector<2,T> > &points)
{
  int dir=IsConvex(points);
  if (dir!=0) return dir;
  return Sign(PolygonArea(points));
}

//try every 3 adjecent point to see if they can form a triangle
//input must be counter-clockwise
template <typename T>
static void PolygonToTriangles(const vector<Vector<2,T> > &points, vector<Vector3i> &triangles)
{
  T dir=PolygonOrdering(points);
  triangles.clear();
  vector<int> polygon;
  for (zuint i=0; i<points.size(); i++) polygon.push_back(i);
  while(polygon.size()>3)
  {
    int psize=polygon.size();
    int best=-1;
    double maxcostheta=-2;
    for (int cur=0;cur<psize;cur++)
    {
      int t0=polygon[cur],t1=polygon[(cur+1)%psize],t2=polygon[(cur-1+psize)%psize];

      //cannot be on the same line
      //this is the 3rd element of cross product result
      T nor2 = Cross(points[t1]-points[t0],points[t2]-points[t0]);
      if (nor2*dir<EPSILON) //on same line
        continue;

      //no other points should be inside
      bool no_point_inside=true;
      for (vector<int>::iterator li=polygon.begin();li!=polygon.end();li++)
      {
        if (*li==t0 || *li==t1 || *li==t2) continue;
        if (PointInTriangle2D(points[t0],points[t1],points[t2],points[*li]))
        {
          no_point_inside=false;
          break;
        }
      }
      if (!no_point_inside)//some point in side
        continue;

      //find mincostheta of three angles to evaluate if the triangle is "good" or not
      //the largest angle of a triangle should be small <-> the mincostheta should be large
      Vector<2,T> e1(points[t2]-points[t0]);
      Vector<2,T> e2(points[t1]-points[t0]);
      Vector<2,T> e3(points[t2]-points[t1]);
      e1.Normalize();e2.Normalize();e3.Normalize();
      T costheta0 = FastDot(e1,e2);
      T costheta1 = -FastDot(e2,e3);
      T costheta2 = FastDot(e1,e3);
      T costheta=Min(costheta0,costheta1);
      if (costheta>costheta2) costheta=costheta2;

      if (costheta>maxcostheta)
      {best=cur;maxcostheta=costheta;}
    }
    //form triangle
    triangles.push_back(Vector3i(polygon[best],polygon[(best+1)%psize],polygon[(best-1+psize)%psize]));
    polygon.erase(polygon.begin()+best);
  }
  triangles.push_back(Vector3i(polygon[0],polygon[1],polygon[2]));
}

//project vertex to a 2D plane which is defined by PCA of these points
template <int N, typename T>
static Matrix<N,N,T> ProjectTo2D(const vector<Vector<N,T> > &t, vector<Vector<2,T> > &res)
{
  //project to a plane
  Matrix<N,N,T> evector;
  Vector<N,T> evalue;
  PCA<N,T>(t,evector,evalue);
  res.clear();
  res.reserve(t.size());
  for (zuint i=0; i<t.size(); i++) {
    Vector<N,T> tmp=evector*t[i];
    res.push_back(Vector<2,T>(tmp[0],tmp[1]));
  }
  return evector;
}

//polygon area
//positive: polygon vertices are in counter-clockwise ordering
//negative: polygon vertices are in clockwise ordering
template <typename T>
static T PolygonArea(const vector<Vector<2,T> > &t) {
  //sum the areas
  T area=0;
  for (zuint i=0; i<t.size(); i++)
    area+=Cross(t[i],t[(i+1)%t.size()]);
  area/=2;
  return area;
}

static const int PARALLEL = 0;
static const int COINCIDENT = 1;
static const int NOT_INTERSECTING = 2;
static const int INTERSECTING = 3;
template<typename T>
static int LineSegmentIntersect(const Vector<2, T> &s0, const Vector<2, T> &e0,
                  const Vector<2, T> &s1, const Vector<2, T> &e1,
                  Vector<2, T> &intersection) {
  float denom = ((e1[1] - s1[1])*(e0[0] - s0[0])) - ((e1[0] - s1[0])*(e0[1] - s0[1]));
  float nume_a = ((e1[0] - s1[0])*(s0[1] - s1[1])) - ((e1[1] - s1[1])*(s0[0] - s1[0]));
  float nume_b = ((e0[0] - s0[0])*(s0[1] - s1[1])) - ((e0[1] - s0[1])*(s0[0] - s1[0]));

  if(Abs(denom) < EPS) {
    if(Abs(nume_a) < EPS && Abs(nume_b) < EPS) {
      return COINCIDENT;
    }
    return PARALLEL;
  }

  float ua = nume_a / denom;
  float ub = nume_b / denom;

  if(ua >= 0.0f && ua <= 1.0f && ub >= 0.0f && ub <= 1.0f) {
    // Get the intersection point.
    intersection[0] = s0[0] + ua*(e0[0] - s0[0]);
    intersection[1] = s0[1] + ua*(e0[1] - s0[1]);
    return INTERSECTING;
  }
  return NOT_INTERSECTING;
}


static void CreateSphere(SimpleMesh &mesh, int levels=5);

/// Calculate angle between 2 vectors.
/// angle is from v1 to v2, counter closewise.
/// Return a value from [0, 2PI).
template<typename T>
static double AngleFrom2Vectors(Vector<3,T> v1, Vector<3,T> v2)
{
  v1.Normalize();
  v2.Normalize();
  double cosalpha = FastDot(v1,v2);
  Vector<3,T> cross = Cross(v1,v2);
  double sinalpha = cross.Len();

  if (Within<double>(1.0-EPSILON5, sinalpha, 1.0+EPSILON5)) return 0;
  if (Within<double>(-1.0-EPSILON5, sinalpha, -1.0+EPSILON5)) return C_PI;

  Vector<3,T> aver = (v1+v2).Normalized();
  Vector<3,T> crossaver = Cross(v1,aver);
  if (FastDot(cross, crossaver) > 0)  // less then PI
    return SafeACos(cosalpha);
  else
    return C_2PI - SafeACos(cosalpha);
}

/// Calculate angle from sin and cos value of it.
/// Return a value from [0, 2PI).
/// sin and cos value will be clamp to [-1,1]
static double AngleFromSinCos(double sinalpha, double cosalpha);

/// Generate texture coordinate to a near-plane mesh
static Matrix3x3f GenTexCoordFlat(Mesh<3> &mesh, zuint group, double pixel_per_one, zuint &u_size, zuint &v_size);

/// Rasterize position to image according to texture coordinate
static bool RasterizePosToImage(const Mesh<3> &mesh, zuint group, Array<2,Vector3f> &pos_img, Array2uc &pos_taken);
};
}
